#include "lenstraymagazine.h"

LensTrayMagazine::LensTrayMagazine(LensTrayModuleConfig *ltConfig, LensTrayModuleState *ltState, QObject *parent)
    : Worker(WorkerName::LensTrayMagazine, parent), ltConfig(ltConfig), ltState(ltState)
{
    connect(ltConfig, &LensTrayModuleConfig::ltMagTrayCountChanged, this, &LensTrayMagazine::resetMagTrayState);
}

void LensTrayMagazine::initEventHandle()
{
    ltMagZ = MotionElementContainer::getIns()->getItem<SingleAxis>(SAxisModuleName::LTMagZ);
    ltKicker = MotionElementContainer::getIns()->getItem<SCCylinder>(CylName::LTKicker);
    ltMagTraySensor = MotionElementContainer::getIns()->getItem<SCDI>(DiName::LTMagTraySNR);
    ltMagSensor = MotionElementContainer::getIns()->getItem<SCDI>(DiName::LTMagSNR);
}

void LensTrayMagazine::startEventHandle(const WorkerEvent &event)
{
    Q_UNUSED(event);
    inLoadUnloadTrayLoopTestMode = false;
    isBhStopped = false;
    setIsLoadingUnloadingTray(false);
    pushEvent(WorkerEvent("retractKicker", QVariant(), 3000));
}

void LensTrayMagazine::allWorkerStopped()
{
    isBhStopped = false;
}

void LensTrayMagazine::purgeEventHandle()
{
    purgeEventHandled();
}

void LensTrayMagazine::resetEventHandle()
{
    isBhStopped = false;
    setIsLoadingUnloadingTray(false);
}

void LensTrayMagazine::homeWorker(bool moveToReadyPosAfterHome)
{
    ltKicker->set(false);
    ltMagZ->axis()->home();

    if (moveToReadyPosAfterHome)
    {
        moveToKickTrayPos(0);
    }
}

void LensTrayMagazine::moveToNextRawTrayPos(bool stayThere)
{
    checkIsInit();

    if (m_isLoadingUnloadingTray)
    {
        QElapsedTimer timer;
        timer.start();
        while (m_isLoadingUnloadingTray)
        {
            if (timer.elapsed() > 10000)
            {
                qCWarning(logCate()) << tr("Waiting loading unloading tray...");
                timer.restart();
            }
            if (hasAbortEvent())
            {
                return;
            }
            QThread::msleep(50);
        }
    }

    if (stayThere)
    {
        setIsLoadingUnloadingTray(true);
    }

    for (int i = 0; i < ltConfig->ltMagTrayCount(); i++)
    {
        if (isBhStopped)
        {
            handleStop();
            return;
        }

        int trayState = ltState->getMagTrayState(i);
        if (trayState == LensTrayModuleState::Init)
        {
            moveToKickTrayPos(i);
            if (ltMagTraySensor->waitState(true, 150))
            {
                ltState->setMagTrayState(i, LensTrayModuleState::RawTray);
                emit sendEvent(WorkerName::LensTrayBuffer, WorkerEvent("ltMagArrivedNextRawTrayPos"));
                return;
            }
            else
            {
                ltState->setMagTrayState(i, LensTrayModuleState::NoTray);
            }
        }
        else if (trayState == LensTrayModuleState::RawTray)
        {
            moveToKickTrayPos(i);
            emit sendEvent(WorkerName::LensTrayBuffer, WorkerEvent("ltMagArrivedNextRawTrayPos"));
            return;
        }
    }

    if (ltState->lensTrayBufferHasTray() && ltState->lensTrayLoaderHasTray())
    {
        return;
    }

    setIsLoadingUnloadingTray(false);
    pushEvent(WorkerEvent("changeMagazine", QVariant(), 2000));
    QVariantList args;
    args << stayThere;
    pushEvent(WorkerEvent("moveToNextRawTrayPos", args, 2000));
}

void LensTrayMagazine::moveToFirstInUseTrayPos(bool stayThere)
{
    checkIsInit();

    if (m_isLoadingUnloadingTray)
    {
        QElapsedTimer timer;
        timer.start();
        while (m_isLoadingUnloadingTray)
        {
            if (timer.elapsed() > 10000)
            {
                qCWarning(logCate()) << tr("Waiting loading unloading tray...");
                timer.restart();
            }
            if (hasAbortEvent())
            {
                return;
            }
            QThread::msleep(50);
        }
    }

    if (stayThere)
    {
        setIsLoadingUnloadingTray(true);
    }

    for (int i = 0; i < ltConfig->ltMagTrayCount(); i++)
    {
        if (ltState->getMagTrayState(i) == LensTrayModuleState::InUse)
        {
            moveToRetrieveTrayPos(i);
            emit sendEvent(WorkerName::LensTrayLoader, WorkerEvent("ltMagArrivedFirstInUseTrayPos"));
            return;
        }
    }
    qCWarning(logCate()) << tr("Did not find InUse tray pos! Try to find NoTray pos instead...");
    for (int i = 0; i < ltConfig->ltMagTrayCount(); i++)
    {
        if (ltState->getMagTrayState(i) == LensTrayModuleState::NoTray)
        {
            moveToRetrieveTrayPos(i);
            emit sendEvent(WorkerName::LensTrayLoader, WorkerEvent("ltMagArrivedFirstInUseTrayPos"));
            return;
        }
    }
    qCWarning(logCate()) << tr("Did not find NoTray tray pos! Try to scan and detect NoTray pos instead...");
    for (int i = 0; i < ltConfig->ltMagTrayCount(); i++)
    {
        int trayState = ltState->getMagTrayState(i);
        if (trayState == LensTrayModuleState::Init)
        {
            moveToKickTrayPos(i);
            if (ltMagTraySensor->waitState(true, 150))
            {
                ltState->setMagTrayState(i, LensTrayModuleState::RawTray);
                continue;
            }
            else
            {
                ltState->setMagTrayState(i, LensTrayModuleState::NoTray);
                moveToRetrieveTrayPos(i);
                emit sendEvent(WorkerName::LensTrayLoader, WorkerEvent("ltMagArrivedFirstInUseTrayPos"));
                return;
            }
        }
    }
    throw SilicolAbort(tr("Did not find neither InUse tray pos nor NoTray pos!"), EX_LOCATION);
}

void LensTrayMagazine::kickTray()
{
    checkIsInit();

    ltKicker->set(true);
}

void LensTrayMagazine::retractKicker()
{
    checkIsInit();

    ltKicker->set(false);
}

void LensTrayMagazine::setCurrentTrayState(MagTrayState::State state)
{
    if (currentMagTrayPosType != KickTrayPos && currentMagTrayPosType != RetrieveTrayPos)
    {
        throw SilicolAbort(
            tr("currentMagTrayPosType is neither KickTrayPos nor RetrieveTrayPos while setCurrentTrayState!"),
            EX_LOCATION);
    }
    ltState->setMagTrayState(currentTrayIndex, state);
}

void LensTrayMagazine::changeMagazine()
{
    checkIsInit();

    if (isBhStopped)
    {
        handleStop();
        return;
    }

    currentTrayIndex = -1;
    currentMagTrayPosType = -1;
    ltMagZ->moveTo(SAxisPosName::ChangeMagazinePos);

    if (isBhStopped)
    {
        handleStop();
        return;
    }

    if (!inLoadUnloadTrayLoopTestMode)
    {
        UIOperation::getIns()->getUIResponse("", tr("Please change lens tray magazine, then click 'Yes' button!"),
                                             MsgBoxIcon::Information, YesBtn);
        while (!ltMagSensor->get(-1))
        {
            if (hasAbortEvent())
            {
                return;
            }
            if (isBhStopped)
            {
                handleStop();
                return;
            }

            UIOperation::getIns()->getUIResponse("", tr("LensTrayMagazineSensor did not detect signal!"),
                                                 MsgBoxIcon::Information, YesBtn);
        }
    }
    ltState->resetMagTrayState(ltConfig->ltMagTrayCount());
}

void LensTrayMagazine::setIsLoadingUnloadingTray(bool value)
{
    m_isLoadingUnloadingTray = value;
}

void LensTrayMagazine::bhStopped()
{
    isBhStopped = true;
    if (!isRunning())
    {
        handleStop();
    }
}

void LensTrayMagazine::reqMoveToKickTrayPos(int trayIndex)
{
    checkIsInit();
    getValidUIIndex(trayIndex);
    moveToKickTrayPos(trayIndex);
}

void LensTrayMagazine::reqMoveToRetrieveTrayPos(int trayIndex)
{
    checkIsInit();
    getValidUIIndex(trayIndex);
    moveToRetrieveTrayPos(trayIndex);
}

void LensTrayMagazine::setInLoadUnloadTrayLoopTestMode(bool value)
{
    inLoadUnloadTrayLoopTestMode = value;
}

void LensTrayMagazine::resetMagTrayState()
{
    ltState->resetMagTrayState(ltConfig->ltMagTrayCount());
}

void LensTrayMagazine::getValidUIIndex(int &trayIndex)
{
    validateIndex(trayIndex, ltConfig->ltMagTrayCount(), "Tray index exceeded limit!");
}

void LensTrayMagazine::moveToKickTrayPos(int trayIndex)
{
    ltKicker->set(false);
    SingleAxisPos *kickFirstTrayPos = ltMagZ->getPos<SingleAxisPos>(SAxisPosName::KickFirtTrayPos);
    SingleAxisPos *kickLastTrayPos = ltMagZ->getPos<SingleAxisPos>(SAxisPosName::KickLastTrayPos);
    double delta = (kickLastTrayPos->pos() - kickFirstTrayPos->pos()) / (ltConfig->ltMagTrayCount() - 1);
    double targetPos = kickFirstTrayPos->pos() + delta * trayIndex;
    ltMagZ->axis()->absMove(targetPos, true, kickFirstTrayPos->precision());
    currentMagTrayPosType = KickTrayPos;
    currentTrayIndex = trayIndex;
}

void LensTrayMagazine::moveToRetrieveTrayPos(int trayIndex)
{
    ltKicker->set(false);
    SingleAxisPos *retrieveFirstTrayPos = ltMagZ->getPos<SingleAxisPos>(SAxisPosName::RetrieveFirstTrayPos);
    SingleAxisPos *retrieveLastTrayPos = ltMagZ->getPos<SingleAxisPos>(SAxisPosName::RetrieveLastTrayPos);
    double delta = (retrieveLastTrayPos->pos() - retrieveFirstTrayPos->pos()) / (ltConfig->ltMagTrayCount() - 1);
    double targetPos = retrieveFirstTrayPos->pos() + delta * trayIndex;
    ltMagZ->axis()->absMove(targetPos, true, retrieveFirstTrayPos->precision());
    currentMagTrayPosType = RetrieveTrayPos;
    currentTrayIndex = trayIndex;
}

void LensTrayMagazine::handleStop()
{
    clearEvent();
    stopEventHandled();
    emit sendEvent(WorkerName::LensTrayBuffer, WorkerEvent("ltMagStopped"));
}
